#include <surftrack.h>
#include <Surfaces.h>
#include <vec.h>
#include <vector>
#include "mex.h"

#pragma comment(lib, "eltopo2d")
#pragma comment(lib, "imager2d")
#pragma comment(lib, "imath2d")
#pragma comment(lib, "cblas_d")

using namespace std;

eltopo2d::SurfTrack *surf;
Surfaces *mine;
double *velocity;

Vec2d advectPoint(Vec2d start, double timestep, int index)
{
	return start + timestep * Vec2d(velocity[2*index+0], velocity[2*index+1]);
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    double *conf, *vert, *edge, *oconf, *overt, *oedge;
    
    conf = mxGetPr(prhs[0]);
    
    if(conf[0] == 1)
    {
        vert = mxGetPr(prhs[1]);
        edge = mxGetPr(prhs[2]);
        
        eltopo2d::SurfTrackInitializationParameters eltopoParams;
        eltopoParams.m_min_edge_length = conf[1];
        eltopoParams.m_max_edge_length = conf[2];
        eltopoParams.m_merge_proximity_epsilon = conf[3];
        eltopoParams.m_max_area_change = conf[4];
        eltopoParams.m_allow_topology_changes = true;
        eltopoParams.m_verbose = false;
        eltopoParams.m_collision_safety = true;
        eltopoParams.m_perform_improvement = true;
        
        int numV, numE;
        vector<Vec2d> verts;
        vector<Vec2ui> edges;
        vector<double> masses;
        
        numV = conf[5];
        numE = conf[6];
        
        for(int i=0; i<numV; i++)
        {
            verts.push_back(Vec2d(vert[i*2], vert[i*2+1]));
            masses.push_back(1);
        }
        
        for(int i=0; i<numE; i++)
        {
            edges.push_back(Vec2ui(edge[i*2], edge[i*2+1]));            
        }
        
        surf = new eltopo2d::SurfTrack(verts, edges, masses, eltopoParams);
        
        surf->improve_mesh();
        surf->rebuild_static_broad_phase();
        
        mine = new Surfaces(surf);
        
    }else{
        
        int numV = conf[1];
        double timestep = conf[2];
        velocity = mxGetPr(prhs[1]);
        
        mine->advect(advectPoint, timestep);
        
    }
    
    int nV, nE;
    
    nV = surf->m_positions.size();
    nE = surf->m_mesh.edges.size();
    
    plhs[0] = mxCreateDoubleMatrix(2, 1, mxREAL);
    plhs[1] = mxCreateDoubleMatrix(nV*2, 1, mxREAL);
    plhs[2] = mxCreateDoubleMatrix(nE*2, 1, mxREAL);
    
    oconf = mxGetPr(plhs[0]);
    overt = mxGetPr(plhs[1]);
    oedge = mxGetPr(plhs[2]);
    
    oconf[0] = nV;
    oconf[1] = nE;
    
    for(int i=0; i<nV; i++)
    {
        overt[i*2+0] = surf->m_positions[i][0];
        overt[i*2+1] = surf->m_positions[i][1];
    }
    
    for(int i=0; i<nE; i++)
    {
        oedge[i*2+0] = surf->m_mesh.edges[i][0];
        oedge[i*2+1] = surf->m_mesh.edges[i][1];
    }
    
	return;
}